#include "angle.h"

/*****可供调整的宏定义区******/

// 定义计算周期，2ms，可调
#define DELTA_T 0.002f

// 定义一阶低通滤波的alpha值，用于平滑加速度值，可调
#define alpha 0.25f
// 滤波尖峰阈值，可调
#define GYRO_DEADZONE_DPS 0.4f

// 四元数解算里用的参数，可调
#define param_kp 0.21
#define param_ki 0.00105

// 定义圆周率精度
#define M_PI 3.1415926f

// 定义零漂结构体
static gyro_offset_t gyro_offset;

// 定义算法用--弧度制的角速度-线加速度数据结构体
GyAc_param_t imu_data;

// 定义现实物理角全局结构体变量，角速度单位为 °/s
GyAc_param_t reality;

// 定义四元数结构体变量
static quater_param_t q_info = {1, 0, 0, 0};

//定义欧拉角结构体变量，单位是度
euler_param_t euler_angle;

// 误差积分
static float i_error_x = 0;
static float i_error_y = 0;
static float i_error_z = 0;

// 中转数据，内部函数调用
static float temp_gyro_x, temp_gyro_y, temp_gyro_z, imu_acc_x, imu_acc_y, imu_acc_z;

/************************硬件交互层************************/

/**
 * @brief 获取陀螺仪的角速度值
 * @note 如果要修改IMU型号，只需修改此函数中的获取角速度的API即可
 */
void Get_Gyro_IMU()
{
    // 改这条
    imu660ra_get_gyro();
    temp_gyro_x = imu660ra_gyro_transition(imu660ra_gyro_x);
    temp_gyro_y = imu660ra_gyro_transition(imu660ra_gyro_y);
    temp_gyro_z = imu660ra_gyro_transition(imu660ra_gyro_z);
}

/**
 * @brief 获取陀螺仪的加速度值
 * @note 如果要修改IMU型号，只需修改此函数中的获取加速度的API即可
 */
void Get_Acc_IMU()
{
    // 改这条
    imu660ra_get_acc();
    imu_acc_x = imu660ra_acc_transition(imu660ra_acc_x);
    imu_acc_y = imu660ra_acc_transition(imu660ra_acc_y);
    imu_acc_z = imu660ra_acc_transition(imu660ra_acc_z);
}

/**********************内部算法层***************************/

/**
 * @brief 快速开方函数
 * @param x
 * @return 返回1/√x
 * @note 此文件内部调用
 * @example float result = Sqrt_Fast(4.0f); // result将会是0.5
 */
static float Sqrt_Fast(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    int32_t i = *(int32_t *)&y;
    i = 0x5f3759df - (i >> 1);
    y = *(float *)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}

/**
 * @brief 数据滤波,转化,同步:加速度一阶低通 + 陀螺仪去零漂 + 阈值滤波 + 角速度单位转弧度+同步所有数据到imu_data中。
 * @note 此文件内部调用
 * @example Get_FilterValues();
 */
static void Get_FilteredValues(void)
{
    static float last_acc_filter[3] = {0, 0, 0};

    // 对加速度进行一阶低通滤波
    imu_data.acc_x = alpha * imu_acc_x + (1 - alpha) * last_acc_filter[0];
    imu_data.acc_y = alpha * imu_acc_y + (1 - alpha) * last_acc_filter[1];
    imu_data.acc_z = alpha * imu_acc_z + (1 - alpha) * last_acc_filter[2];

    // 更新上一次的加速度数据
    last_acc_filter[0] = imu_data.acc_x;
    last_acc_filter[1] = imu_data.acc_y;
    last_acc_filter[2] = imu_data.acc_z;

    // 去零漂后同步到真实物理角全局变量中，角加速度不进行阈值滤波，保留最真实的数据
    // 在调用此函数前，会调用Get_Gyro_IMU()，Get_Acc_IMU()获取最新的陀螺仪角速度，线加速度原始值，然后在此函数里对原始角速度进行去零漂的操作,对加速度进行低通滤波
    // 做完上述操作，再存进reality结构体中
    reality.gyro_x = temp_gyro_x - gyro_offset.x_data;
    reality.gyro_y = temp_gyro_y - gyro_offset.y_data;
    reality.gyro_z = temp_gyro_z - gyro_offset.z_data;

    // 阈值滤波(抑制微小抖动)
    if (reality.gyro_x > -GYRO_DEADZONE_DPS && reality.gyro_x < GYRO_DEADZONE_DPS)
    {
        reality.gyro_x = 0;
    }

    if (reality.gyro_y > -GYRO_DEADZONE_DPS && reality.gyro_y < GYRO_DEADZONE_DPS)
    {
        reality.gyro_y = 0;
    }

    if (reality.gyro_z > -GYRO_DEADZONE_DPS && reality.gyro_z < GYRO_DEADZONE_DPS)
    {
        reality.gyro_z = 0;
    }

    // 把角度值转化为弧度制
    imu_data.gyro_x = reality.gyro_x * M_PI / 180.0f;
    imu_data.gyro_y = reality.gyro_y * M_PI / 180.0f;
    imu_data.gyro_z = reality.gyro_z * M_PI / 180.0f;
}

/**
 * @brief 更新姿态四元数
 * @note 此函数的核心算法有问题，待定
 * @param gx    x轴方向角速度分量
 * @param gy    y轴方向角速度分量
 * @param gz    z轴方向角速度分量
 * @param ax    x轴方向加速度分量
 * @param ay    y轴方向加速度分量
 * @param az    z轴方向加速度分量
 */
static void Update_AHRS_IMU(float gx, float gy, float gz, float ax, float ay, float az)
{
    // 测量周期的一半。用于四元数微分方程
    float half_t = 0.5 * DELTA_T;
    // 当前载体坐标系上的重力单位加速度，四元数解算获得
    float vx = 0, vy = 0, vz = 0;
    // 四元数解算值与加速度计测量值的误差
    float ex = 0, ey = 0, ez = 0;
    // 从四元数结构体中获取当前的四元数分量
    float q0 = q_info.q0;
    float q1 = q_info.q1;
    float q2 = q_info.q2;
    float q3 = q_info.q3;

    // 归一化加速度数据后获得单位向量
    float norm = Sqrt_Fast(ax * ax + ay * ay + az * az);
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;

    // 根据当前四元数姿态估算重力分量与实际测量的重力分量对比
    vx = 2 * (q1 * q3 - q0 * q2);
    vy = 2 * (q0 * q1 + q2 * q3);
    vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

    // 计算误差向量用以修正陀螺仪数据
    ex = ay * vz - az * vy;
    ey = az * vx - ax * vz;
    ez = ax * vy - ay * vx;

    // 使用PI控制器修正陀螺仪数据减少误差
    i_error_x += ex * half_t;
    i_error_y += ey * half_t;
    i_error_z += ez * half_t;
    gx = gx + param_kp * ex + param_ki * i_error_x;
    gy = gy + param_kp * ey + param_ki * i_error_y;
    gz = gz + param_kp * ez + param_ki * i_error_z;

    // 使用一阶龙格库塔方法求解四元数微分方程与更新四元数
    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * half_t;
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * half_t;
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * half_t;
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * half_t;

    // 归一化四元数
    norm = Sqrt_Fast(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q_info.q0 = q0 * norm;
    q_info.q1 = q1 * norm;
    q_info.q2 = q2 * norm;
    q_info.q3 = q3 * norm;
}

/******************任务层(对外调用的API)**********************/

/**
 * @brief 获取零漂值，存在gyro_offset结构体中
 *
 */
void Init_Gyro_Offset(void)
{
    gyro_offset.x_data = 0;
    gyro_offset.y_data = 0;
    gyro_offset.z_data = 0;

    for (int i = 0; i < 1000;)
    {
        // 获取陀螺仪数据
        Get_Gyro_IMU();

        // 舍去离谱数值
        if (temp_gyro_x > -20 && temp_gyro_x < 20 &&
            temp_gyro_y > -20 && temp_gyro_y < 20 &&
            temp_gyro_z > -20 && temp_gyro_z < 20)
        {
            i++;
            gyro_offset.x_data += temp_gyro_x;
            gyro_offset.y_data += temp_gyro_y;
            gyro_offset.z_data += temp_gyro_z;
        }

        system_delay_ms(1);
    }
    // 获取零漂平均值
    gyro_offset.x_data /= 1000;
    gyro_offset.y_data /= 1000;
    gyro_offset.z_data /= 1000;
}

/**
 * @brief 四元数转换成欧拉角(单位:角度)
 * @note  中断中固定周期调用
 * @example Get_Angles_IMU();
 */
void Get_Angles_IMU(void)
{
    // 获取陀螺仪最新的角速度和加速度值
    Get_Gyro_IMU();
    Get_Acc_IMU();

    // 滤波与转化
    Get_FilteredValues();

    // 更新四元数
    Update_AHRS_IMU(imu_data.gyro_x, imu_data.gyro_y, imu_data.gyro_z, imu_data.acc_x, imu_data.acc_y, imu_data.acc_z);

    float q0 = q_info.q0;
    float q1 = q_info.q1;
    float q2 = q_info.q2;
    float q3 = q_info.q3;

    // 由四元数计算欧拉角

    //夹紧sin值域   
    float sp = -2 * q1 * q3 + 2 * q0 * q2;
    if (sp > 1.0f)  sp = 1.0f;
    if (sp < -1.0f) sp = -1.0f;
    euler_angle.pitch = asin(sp) * 180 / M_PI;
    // - 180;       AI说要去掉-180，我也不知道为什么，反正也不需要用到pitch
    euler_angle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI;
    euler_angle.yaw = atan2f(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI;
}

// 习惯调用前后标志位初始化
uint8_t now_angle_flag = 1;
double now_angle = 0;

/**
 * @brief 积分z轴角速度，计算相对转角（单位：度）。
 * @note 需周期性调用，常用于累计小车自某一时刻起的相对转动角度。
 * @details
 * - 第一次调用或标志位now_angle_flag为1时，累计角度清零。
 * - 每次调用将当前z轴角速度（reality.gyro_z）乘以采样周期（2ms）累加到now_angle。
 * - 适合短时相对角度测量，长时间会有漂移。
 * @see euler_angle.yaw 获取绝对航向角
 */
void relative_angle()
{
    if (now_angle_flag == 1)
    {
        now_angle = 0;
        now_angle_flag = 0;
    }
    now_angle = now_angle + reality.gyro_z * 0.002;
}
